package cn.com.fhc.ros

import cn.com.fhc.ros.messages.Message
import cn.com.fhc.ros.services.ServiceRequest
import cn.com.fhc.ros.services.ServiceResponse
import jakarta.websocket.*
import java.io.IOException
import java.io.StringReader
import java.net.URI
import javax.json.Json
import javax.json.JsonObject


/**
 * The Ros object is the main connection point to the rosbridge server. This
 * object manages all communication to-and-from ROS.
 *
 * @author fanhoucheng@sina.com
 * @version April 1, 2014
 */
@ClientEndpoint
class Ros() {


    private  var hostname: String = "localhost"
    private var port: Int = 9090
    private var protocol: WebSocketType = WebSocketType.ws

    // active session (stored upon connection)
    private var session: Session? = null

    // used throughout the library to create unique IDs for requests.
    private var idCounter: Long = 0

    // keeps track of callback functions for a given topic
    private var topicCallbacks = HashMap<String, MutableList<TopicCallback>>()

    // keeps track of callback functions for a given service request
    private var serviceCallbacks = HashMap<String, ServiceResponseCallback>()

    // keeps track of callback functions for a given advertised service
    private var callServiceCallbacks = HashMap<String, ServiceRequestCallback>()

    // keeps track of handlers for this connection
    private var connCallbacks: MutableList<ConnectCallback> = mutableListOf()

    constructor(hostname: String): this() {
        this.hostname = hostname
    }

    constructor (hostname: String, port: Int): this() {
        this.hostname = hostname
        this.port = port
    }


    constructor(hostname: String, port: Int, protocol: WebSocketType): this() {
        this.hostname = hostname
        this.port = port
        this.protocol = protocol
        this.session = null
        this.idCounter = 0
        this.topicCallbacks = HashMap()
        this.serviceCallbacks = HashMap()
        this. callServiceCallbacks = HashMap()
        this. connCallbacks = mutableListOf()
    }
    /**
     * Get the full URL this client is connecting to.
     *
     * @return
     */
    private val url: String
        get() = (protocol.toString() + "://" + hostname + ":"
                + port)

    /**
     * Get the next unique ID number for this connection.
     *
     * @return The next unique ID number for this connection.
     */
    fun nextId(): Long {
        return idCounter++
    }

    /**
     * Add a handler to this connection. This handler is called when the
     * associated events occur.
     *
     * @param handler
     * The handler to add.
     */
    fun addConnectCallback(handler: ConnectCallback) {
        connCallbacks.add(handler)
    }

    /**
     * Attempt to establish a connection to rosbridge. Errors are printed to the
     * error output stream.
     *
     * @return Returns true if the connection was established successfully and
     * false otherwise.
     */
    fun connect(cb: ConnectCallback): Boolean {
        addConnectCallback(cb)
        return try {
            // create a WebSocket connection here
            val uri: URI = URI(url)
            ContainerProvider.getWebSocketContainer()
                .connectToServer(this, uri)
            true
        } catch (e: Exception) {
            for (callback: ConnectCallback in connCallbacks) {
                callback.handleError(session, e)
            }
            false
        }
    }

    /**
     * Disconnect the connection to rosbridge. Errors are printed to the error
     * output stream.
     *
     * @return Returns true if the disconnection was successful and false
     * otherwise.
     */
    fun disconnect(): Boolean {
        if (isConnected) {
            try {
                session!!.close()
                return true
            } catch (e: IOException) {
                e.printStackTrace()
            }
        }
        // could not disconnect cleanly
        return false
    }

    /**
     * Check if there is a connection to rosbridge.
     *
     * @return If there is a connection to rosbridge.
     */
    val isConnected: Boolean
        get() {
            return session != null && session!!.isOpen
        }

    /**
     * This function is called once a successful connection is made.
     *
     * @param session
     * The session associated with the connection.
     */
    @OnOpen
    fun onOpen(session: Session?) {
        // store the session
        this.session = session

        // call the handlers
        for (handler: ConnectCallback in this.connCallbacks) handler.handleConnection(session)
    }

    /**
     * This function is called once a successful disconnection is made.
     *
     * @param session
     * The session associated with the disconnection.
     */
    @OnClose
    fun onClose(session: Session?) {
        // remove the session
        this.session = null

        // call the handlers
        for (handler: ConnectCallback in this.connCallbacks)  handler.handleDisconnection(session)
    }

    /**
     * This function is called if an error occurs.
     *
     * @param session
     * The session for the error.
     * @param session
     * The session for the error.
     */
    @OnError
    fun onError(session: Session?, t: Throwable?) {
        // call the handlers
        for (handler: ConnectCallback in this.connCallbacks) handler.handleError(session, t)
    }

    /**
     * This method is called once an entire message has been read in by the
     * connection from rosbridge. It will parse the incoming JSON and attempt to
     * handle the request appropriately.
     *
     * @param message
     * The incoming JSON message from rosbridge.
     */
    @OnMessage
    fun onMessage(message: String) {
        try {
            // parse the JSON
            val jsonObject: JsonObject = Json
                .createReader(StringReader(message)).readObject()

            // check for compression
            val op: String = jsonObject.getString(FIELD_OP)
            if ((op == OP_CODE_PNG)) {
//				String data = jsonObject.getString(FIELD_DATA);
//				// decompress the PNG data
//				byte[] bytes = Base64Utils.decode(data.getBytes());
//				Raster imageData = ImageIO
//						.read(new ByteArrayInputStream(bytes)).getRaster();
//
//				// read the RGB data
//				int[] rawData = null;
//				rawData = imageData.getPixels(0, 0, imageData.getWidth(),
//						imageData.getHeight(), rawData);
//				StringBuffer buffer = new StringBuffer();
//				for (int i = 0; i < rawData.length; i++) {
//					buffer.append(Character.toString((char) rawData[i]));
//				}
//
//				// reparse the JSON
//				JsonObject newJsonObject = Json.createReader(
//						new StringReader(buffer.toString())).readObject();
//				handleMessage(newJsonObject);
            } else {
                handleMessage(jsonObject)
            }
        } catch (e:Exception) {
            System.err.println(
                ("[WARN]: Invalid incoming rosbridge protocol: "
                        + message)
            )
        }
    }

    /**
     * Handle the incoming rosbridge message by calling the appropriate
     * callbacks.
     *
     * @param jsonObject
     * The JSON object from the incoming rosbridge message.
     */
    private fun handleMessage(jsonObject: JsonObject) {
        // check for the correct fields
        val op: String = jsonObject.getString(FIELD_OP)
        if ((op == OP_CODE_PUBLISH)) {
            // check for the topic name
            val topic: String = jsonObject.getString(FIELD_TOPIC)

            // call each callback with the message
            val callbacks: MutableList<TopicCallback>? = topicCallbacks[topic]
            if (callbacks != null) {
                val msg = Message(
                    jsonObject.getJsonObject(FIELD_MESSAGE)
                )
                for (cb: TopicCallback in callbacks) {
                    cb.handleMessage(msg)
                }
            }
        } else if ((op == OP_CODE_SERVICE_RESPONSE)) {
            // check for the request ID
            val id: String = jsonObject.getString(FIELD_ID)

            // call the callback for the request
            val cb: ServiceResponseCallback? = serviceCallbacks[id]
            if (cb != null) {
                // check if a success code was given
                val success: Boolean = if (jsonObject
                        .containsKey(FIELD_RESULT)
                ) jsonObject
                    .getBoolean(FIELD_RESULT) else true
                // get the response
                val values: JsonObject = jsonObject
                    .getJsonObject(FIELD_VALUES)
                val response = ServiceResponse(values, success)
                cb.handleServiceResponse(response)
            }
        } else if ((op == OP_CODE_CALL_SERVICE)) {
            // check for the request ID
            val id: String = jsonObject.getString("id")
            val service: String = jsonObject.getString("service")

            // call the callback for the request
            val cb: ServiceRequestCallback? = callServiceCallbacks[service]
            if (cb != null) {
                // get the response
                val args: JsonObject = jsonObject.getJsonObject(FIELD_ARGS)
                val request = ServiceRequest(args)
                request.id = id
                cb.handleServiceCall(request)
            }
        } else {
            System.err.println(
                ("[WARN]: Unrecognized op code: "
                        + jsonObject.toString())
            )
        }
    }

    /**
     * Send the given JSON object to rosbridge.
     *
     * @param jsonObject
     * The JSON object to send to rosbridge.
     * @return If the sending of the message was successful.
     */
    fun send(jsonObject: JsonObject): Boolean {
        // check the connection
        if (isConnected) {
            try {
                // send it as text
                session!!.basicRemote.sendText(jsonObject.toString())
                return true
            } catch (e: IOException) {
                System.err.println(
                    ("[ERROR]: Could not send message: "
                            + e.message)
                )
            }
        }
        // message send failed
        return false
    }

    /**
     * Sends an authorization request to the server.
     *
     * @param mac
     * The MAC (hash) string given by the trusted source.
     * @param client
     * The IP of the client.
     * @param dest
     * The IP of the destination.
     * @param rand
     * The random string given by the trusted source.
     * @param t
     * The time of the authorization request.
     * @param level
     * The user level as a string given by the client.
     * @param end
     * The end time of the client's session.
     */
    fun authenticate(
        mac: String?, client: String?, dest: String?,
        rand: String?, t: Int, level: String?, end: Int
    ) {
        // build and send the rosbridge call
        val call: JsonObject = Json.createObjectBuilder()
            .add(FIELD_OP, OP_CODE_AUTH)
            .add(FIELD_MAC, mac)
            .add(FIELD_CLIENT, client)
            .add(FIELD_DESTINATION, dest)
            .add(FIELD_RAND, rand).add(FIELD_TIME, t)
            .add(FIELD_LEVEL, level)
            .add(FIELD_END_TIME, end).build()
        send(call)
    }

    /**
     * Register a callback for a given topic.
     *
     * @param topic
     * The topic to register this callback with.
     * @param cb
     * The callback that will be called when messages come in for the
     * associated topic.
     */
    fun registerTopicCallback(topic: String, cb: TopicCallback) {
        // check if any callbacks exist yet
        if (!topicCallbacks.containsKey(topic)) {
            topicCallbacks[topic] = mutableListOf()
        }

        // add the callback
        topicCallbacks[topic]?.add(cb)
    }

    /**
     * Deregister a callback for a given topic.
     *
     * @param topic
     * The topic associated with the callback.
     * @param cb
     * The callback to remove.
     */
    fun deregisterTopicCallback(topic: String, cb: TopicCallback) {
        // check if any exist for this topic
        if (topicCallbacks.containsKey(topic)) {
            // remove the callback if it exists
            val callbacks = topicCallbacks[topic]
            if (callbacks != null) {
                if (callbacks.contains(cb)) {
                    callbacks.remove(cb)
                }
                // remove the list if it is empty
                if (callbacks.size == 0) {
                    topicCallbacks.remove(topic)
                }
            }

        }
    }

    /**
     * Register a callback for a given outgoing service call.
     *
     * @param serviceCallId
     * The unique ID of the service call.
     * @param cb
     * The callback that will be called when a service response comes
     * back for the associated request.
     */
    fun registerServiceCallback(serviceCallId: String, cb: ServiceResponseCallback) {
        // add the callback
        serviceCallbacks[serviceCallId] = cb
    }

    /**
     * Register a callback for a given incoming service request.
     *
     * @param serviceName
     * The unique name of the service call.
     * @param cb
     * The callback that will be called when a service request comes
     * in for the associated request.
     */
    fun registerCallServiceCallback(serviceName: String, cb: ServiceRequestCallback) {
        // add the callback
        callServiceCallbacks[serviceName] = cb
    }

    /**
     * Deregister a callback for a given incoming service request.
     *
     * @param serviceName
     * The unique name of the service call.
     */
    fun deregisterCallServiceCallback(serviceName: String) {
        // remove the callback
        callServiceCallbacks.remove(serviceName)
    }

}
